#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <iostream>

int main() {
    // 创建一个简单的KDL链（2个旋转关节）
    KDL::Chain chain;
    chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
                                  KDL::Frame(KDL::Vector(0, 0, 0.1))));
    chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
                                  KDL::Frame(KDL::Vector(0, 0, 0.1))));

    // 关节位置（初始化为0）
    KDL::JntArray joint_positions(chain.getNrOfJoints());
    for (int i = 0; i < chain.getNrOfJoints(); ++i) {
        joint_positions(i) = 0.0;
    }

    // 前向运动学求解器
    KDL::Frame end_effector_pose;
    KDL::ChainFkSolverPos_recursive fk_solver(chain);
    int status = fk_solver.JntToCart(joint_positions, end_effector_pose);

    // 输出结果
    if (status >= 0) {
        std::cout << "成功计算末端执行器位置:\n";
        std::cout << "X: " << end_effector_pose.p.x()
                  << " Y: " << end_effector_pose.p.y()
                  << " Z: " << end_effector_pose.p.z() << std::endl;
    } else {
        std::cerr << "运动学计算失败，错误代码: " << status << std::endl;
        return 1;
    }

    return 0;
}